/**
  ******************************************************************************
  * File Name          : zeus_s3_driver.cpp
  * Description       : zeus_s3_driver program body
  ******************************************************************************
  *
  * Copyright (c) 2019 HopeMotion Co., Ltd. 
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without 
  * modification, are permitted, provided that the following conditions are met:
  *
  * 1. Redistribution of source code must retain the above copyright notice, 
  *    this list of conditions and the following disclaimer.
  * 2. Redistributions in binary form must reproduce the above copyright notice,
  *    this list of conditions and the following disclaimer in the documentation
  *    and/or other materials provided with the distribution.
  * 3. Neither the name of HopeMotion nor the names of other 
  *    contributors to this software may be used to endorse or promote products 
  *    derived from this software without specific written permission.
  * 4. This software, including modifications and/or derivative works of this 
  *    software, must execute solely and exclusively on microcontroller or
  *    microprocessor devices manufactured by or for HopeMotion.
  * 5. Redistribution and use of this software other than as permitted under 
  *    this license is void and will automatically terminate your rights under 
  *    this license. 
  *
  * THIS SOFTWARE IS PROVIDED BY HOPEMOTION AND CONTRIBUTORS "AS IS" 
  * AND ANY EXPRESS, IMPLIED OR STATUTORY WARRANTIES, INCLUDING, BUT NOT 
  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY, FITNESS FOR A 
  * PARTICULAR PURPOSE AND NON-INFRINGEMENT OF THIRD PARTY INTELLECTUAL PROPERTY
  * RIGHTS ARE DISCLAIMED TO THE FULLEST EXTENT PERMITTED BY LAW. IN NO EVENT 
  * SHALL HOPEMOTION OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
  * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, 
  * OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 
  * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 
  * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
  * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
  *
  ******************************************************************************
  */
/* Includes ------------------------------------------------------------------*/
#include <zeus_s3_driver/zeus_s3_driver.h>

std::string _port;
serial::Serial ser;
int _run_mode = 0;
double _steer_max = 15;

static motor_t motor[JOINT_MAX];

static unsigned char step = 0;

static unsigned char ext_rxd[RXD_MAX]; 
static unsigned char ext_txd[TXD_MAX];   

geometry_msgs::Twist twist;     

static float Vx[] = {0.0, 0.0, 0.0, 0.0};
static float Wz[] = {0.0, 0.0, 0.0, 0.0};

/* Private function prototypes -----------------------------------------------*/

/**
  * @brief 
  * @param 
  * @retval 
  */
float deg2rad(float ang)
{
    return ang*M_PI/180;
}

/**
  * @brief 
  * @param 
  * @retval 
  */
float rad2deg(float rad)
{
    return rad*180/M_PI;
}

/**
  * @brief 
  * @param 
  * @retval 
  */
static float ext_get_rxd(uint8_t index)
{
	/* to do */
	float rst = APP_F_NULL;
	
    uint8_t x = index*EXT_DATA_WIDTH + EXT_PTCL_xPTR;
	int16_t data;
	
	data = ext_rxd[x+1];
	data <<= 8;
	data += ext_rxd[x];
	
	rst = data;

    return (rst/100);
	/* end */
}

/**
  * @brief 
  * @param 
  * @retval 
  */
static void ext_set_txd(uint8_t index, float data)
{
	/* to do */
    uint8_t x = index*EXT_DATA_WIDTH + EXT_PTCL_xPTR;
	int16_t tmp = data*100;
  
	ext_txd[x] = (uint8_t)tmp;
	ext_txd[x+1] += (uint8_t)(tmp>>8);
	/* end */
}

/**
  * @brief 
  * @param 
  * @retval 
  */
static uint8_t ext_chkmsg(uint8_t msg[RXD_MAX])
{
    uint32_t cs = APP_NULL;
    uint32_t tmp = APP_NULL;
  
    for(int i = APP_NULL; i< (RXD_MAX-EXT_DATA_WIDTH); i++)
    {
        tmp += msg[i];
    }
  
    cs = msg[EXT_PTCL_C_S+1];
    cs <<= 8;
    cs += msg[EXT_PTCL_C_S];

    return APP_TRUE;
	
    if((cs==tmp)&&(tmp!=0))
    {
        if(msg[EXT_PTCL_CMD]==EXT_CMD_SET_MOTOR_DATA)
        {
            return APP_TRUE;
        }
        else
        {
            return APP_FALSE;
        }
    }
    else
    {
        return APP_FALSE;
    }
}

/**
  * @brief 
  * @param 
  * @retval 
  */
static void motor_stop(void)
{
    uint32_t cs = APP_NULL;
    memset(ext_txd, APP_NULL, TXD_MAX);
    memset(ext_rxd, APP_NULL, RXD_MAX);
    cs = 0; 

    printf("exit\r\n");
    ext_txd[EXT_PTCL_H_S] = EXT_FRAME_HEADER;
    ext_txd[EXT_PTCL_CMD] = EXT_CMD_SET_MOTOR_DATA; 
    ext_txd[EXT_PTCL_IDX] = MOTOR_MIXED_4X4;
    ext_txd[EXT_PTCL_C_S] = 0x3a;

    ser.write(ext_txd,TXD_MAX);
    sleep(4);
}

/**
  * @brief 
  * @param 
  * @retval 
  */
static void ackerman_fk(float vx, float wz)
{
    float R = 0;
    float L = WHEEL_BASE;
    float lw = WHEEL_TRACK;
    float v_l = vx-wz*WHEEL_TRACK/2;
    float v_r = vx+wz*WHEEL_TRACK/2;

    if(wz>0)
    {
        R = fabs(vx/wz);
        Wz[0] = atan(L/(R-lw/2));
        Wz[1] = atan(L/(R+lw/2));
        Vx[0] = v_l*cos(Wz[0]);
        Vx[1] = v_r*cos(Wz[1]);
    }
    else if(wz<0)
    {
        R = fabs(vx/wz);
        Wz[0] = -1*(atan(L/(R+lw/2)));
        Wz[1] = -1*(atan(L/(R-lw/2))); 
        Vx[0] = v_l*cos(Wz[0]);
        Vx[1] = v_r*cos(Wz[1]);
    }
    else
    {
        Wz[0] = 0;
        Wz[1] = 0;

        Vx[0] = vx;
        Vx[1] = vx;
    }

    Vx[2] = v_r;
    Vx[3] = v_l;

    Wz[2] = 0;
    Wz[3] = 0;
}

/**
  * @brief 
  * @param 
  * @retval 
  */
static void ackerman_d_fk(float vx, float wz)
{
    float R = 0;
    float L = WHEEL_BASE/2;
    float lw = WHEEL_TRACK;
    float v_l = vx-wz*WHEEL_TRACK/2;
    float v_r = vx+wz*WHEEL_TRACK/2;


    if(wz>0)
    {
        R = fabs(vx/wz);
        Wz[0] = atan(L/(R-lw/2));
        Wz[1] = atan(L/(R+lw/2));
        Wz[2] = Wz[0]*(-1);
        Wz[3] = Wz[1]*(-1);

        Vx[0] = v_l*cos(Wz[0]);
        Vx[1] = v_r*cos(Wz[1]);
        Vx[2] = Vx[0];
        Vx[3] = Vx[1];
    }
    else if(wz<0)
    {
        R = fabs(vx/wz);
        Wz[0] = -1*(atan(L/(R+lw/2)));
        Wz[1] = -1*(atan(L/(R-lw/2))); 
        Wz[2] = Wz[0]*(-1);
        Wz[3] = Wz[1]*(-1);

        Vx[0] = v_l*cos(Wz[0]);
        Vx[1] = v_r*cos(Wz[1]);
        Vx[2] = Vx[0];
        Vx[3] = Vx[1];
    }
    else
    {
        Wz[0] = 0;
        Wz[1] = 0;
        Wz[2] = 0;
        Wz[3] = 0;

        Vx[0] = vx;
        Vx[1] = vx;
        Vx[2] = vx;
        Vx[3] = vx;
    }
}

/**
  * @brief  接收话题数据
  * @param  消息结构体指针
  * @retval 无
  */
void twistCB(const geometry_msgs::Twist::ConstPtr & msg)
{
    twist.linear.x = msg->linear.x;
    twist.linear.y = msg->linear.y;

    twist.angular.z = msg->angular.z;
}

/**
  * @brief  驱动主函数
  * @param
  * @retval
  */
int main(int argc, char**argv)
{
    ros::init(argc, argv, "zeus_s3_driver");
    ros::NodeHandle nh;
    ros::Rate loop_rate(20);

    uint32_t cs = APP_NULL;
    int i = APP_NULL;

    sensor_msgs::BatteryState battery;
    std_msgs::UInt8 status;
    sensor_msgs::JointState joint_state;
    
    nh.getParam("/zeus_s3_driver/run_mode", _run_mode);
    nh.getParam("/zeus_s3_driver/steer_max", _steer_max);
    nh.getParam("/zeus_s3_driver/port", _port);
    nh.getParam("/zeus_s3_driver/joint_names", joint_state.name);
    nh.getParam("/zeus_s3_driver/joint_position", joint_state.position);
    nh.getParam("/zeus_s3_driver/joint_velocity", joint_state.velocity);

    ros::Publisher pub_battery = nh.advertise<sensor_msgs::BatteryState>("/zeus/battery",5);
    ros::Publisher pub_status = nh.advertise<std_msgs::UInt8>("/zeus/status",1);
    ros::Publisher pub_joints = nh.advertise<sensor_msgs::JointState>("/zeus/joint_states",10);

    ros::Subscriber sub_twist = nh.subscribe("/cmd_vel", 10, twistCB);

    try 
    { 
    //设置串口属性，并打开串口 
        ser.setPort(_port.c_str()); 
        ser.setBaudrate(115200); 
        serial::Timeout to = serial::Timeout::simpleTimeout(5); 
        ser.setTimeout(to); 
        ser.open(); 
    } 
    catch (serial::IOException& e) 
    { 
        ROS_ERROR_STREAM("Unable to open port "); 
        return -1; 
    } 

    while (ros::ok())
    {
        memset(ext_txd, APP_NULL, TXD_MAX);
        memset(ext_rxd, APP_NULL, RXD_MAX);
        cs = 0;
        ext_txd[EXT_PTCL_H_S] = EXT_FRAME_HEADER;  
        ext_txd[EXT_PTCL_CMD] = EXT_CMD_SET_MOTOR_DATA; 
        ext_txd[EXT_PTCL_IDX] = MOTOR_MIXED_4X4;

        switch(_run_mode)
        {
            case 0:  // 四轮同步转向
                if((fabs(twist.linear.x)==0)&&(twist.angular.z))
                {
                    if(((motor[4].fActPOS<(ROTATION-1)*-1)&&(motor[4].fActPOS>(ROTATION+1)*-1))&&
                        ((motor[5].fActPOS>(ROTATION-1))&&(motor[5].fActPOS<(ROTATION+1)))&&
                        ((motor[6].fActPOS>(ROTATION-1))&&(motor[6].fActPOS<(ROTATION+1)))&&
                        ((motor[7].fActPOS<(ROTATION-1)*-1)&&(motor[7].fActPOS>(ROTATION+1)*-1))
                        )
                    {
                        ext_set_txd(EXT_DTX_1, twist.angular.z*(-1));
                        ext_set_txd(EXT_DTX_2, twist.angular.z);
                        ext_set_txd(EXT_DTX_3, twist.angular.z*(-1));
                        ext_set_txd(EXT_DTX_4, twist.angular.z);
                    }
                     
                    ext_set_txd(EXT_DTX_5, ROTATION*(-1));
                    ext_set_txd(EXT_DTX_6, ROTATION);
                    ext_set_txd(EXT_DTX_7, ROTATION);
                    ext_set_txd(EXT_DTX_8, ROTATION*(-1));
		    
                }
		        else
		        {
		            ext_set_txd(EXT_DTX_1, twist.linear.x);
                    ext_set_txd(EXT_DTX_2, twist.linear.x);
                    ext_set_txd(EXT_DTX_3, twist.linear.x);
                    ext_set_txd(EXT_DTX_4, twist.linear.x);

                    ext_set_txd(EXT_DTX_5, rad2deg(twist.angular.z));
                    ext_set_txd(EXT_DTX_6, rad2deg(twist.angular.z));
                    ext_set_txd(EXT_DTX_7, rad2deg(twist.angular.z));
                    ext_set_txd(EXT_DTX_8, rad2deg(twist.angular.z));
		        }
                break;
            case 1:  // 前轮转向
                if ((fabs(twist.linear.x))&&(fabs(twist.angular.z)))
                {
		            if(twist.angular.z > 0)
		            {
		                if(twist.angular.z > deg2rad(_steer_max))
                        {
                            twist.angular.z = deg2rad(_steer_max);
                        }
		            }	
		            if(twist.angular.z < 0)
		            {
                        if(twist.angular.z < deg2rad(_steer_max*(-1)))
                        {
                            twist.angular.z = deg2rad(_steer_max*(-1));
                        }
		            }
                    ackerman_fk(twist.linear.x, twist.angular.z);

                    ext_set_txd(EXT_DTX_1, Vx[0]);
                    ext_set_txd(EXT_DTX_2, Vx[1]);
                    ext_set_txd(EXT_DTX_3, Vx[2]);
                    ext_set_txd(EXT_DTX_4, Vx[3]);

                    ext_set_txd(EXT_DTX_5, rad2deg(Wz[0]));
                    ext_set_txd(EXT_DTX_6, rad2deg(Wz[1]));
                    ext_set_txd(EXT_DTX_7, rad2deg(Wz[2]));
                    ext_set_txd(EXT_DTX_8, rad2deg(Wz[3]));
                }
                else if ((twist.linear.x==0)&&(fabs(twist.angular.z)))
                {
                    if(((motor[4].fActPOS<(ROTATION-1)*-1)&&(motor[4].fActPOS>(ROTATION+1)*-1))&&
                        ((motor[5].fActPOS>(ROTATION-1))&&(motor[5].fActPOS<(ROTATION+1)))&&
                        ((motor[6].fActPOS>(ROTATION-1))&&(motor[6].fActPOS<(ROTATION+1)))&&
                        ((motor[7].fActPOS<(ROTATION-1)*-1)&&(motor[7].fActPOS>(ROTATION+1)*-1))
                        )
                    {
                        ext_set_txd(EXT_DTX_1, twist.angular.z*(-1));
                        ext_set_txd(EXT_DTX_2, twist.angular.z);
                        ext_set_txd(EXT_DTX_3, twist.angular.z*(-1));
                        ext_set_txd(EXT_DTX_4, twist.angular.z);
                    }
                     
                    ext_set_txd(EXT_DTX_5, ROTATION*(-1));
                    ext_set_txd(EXT_DTX_6, ROTATION);
                    ext_set_txd(EXT_DTX_7, ROTATION);
                    ext_set_txd(EXT_DTX_8, ROTATION*(-1));  
                }
                else
                {
                    ext_set_txd(EXT_DTX_1, twist.linear.x);
                    ext_set_txd(EXT_DTX_2, twist.linear.x);
                    ext_set_txd(EXT_DTX_3, twist.linear.x);
                    ext_set_txd(EXT_DTX_4, twist.linear.x);

                    ext_set_txd(EXT_DTX_5, rad2deg(twist.angular.z));
                    ext_set_txd(EXT_DTX_6, rad2deg(twist.angular.z));
                    ext_set_txd(EXT_DTX_7, rad2deg(twist.angular.z));
                    ext_set_txd(EXT_DTX_8, rad2deg(twist.angular.z));  
                }
                break;
            case 2:  // 前轮+后轮转向
                if ((fabs(twist.linear.x))&&(fabs(twist.angular.z)))
                {
                    if(twist.angular.z > 0)
		            {
		                if(twist.angular.z > deg2rad(_steer_max))
                        {
                            twist.angular.z = deg2rad(_steer_max);
                        }
		            }	
		            if(twist.angular.z < 0)
		            {
                        if(twist.angular.z < deg2rad(_steer_max*(-1)))
                        {
                            twist.angular.z = deg2rad(_steer_max*(-1));
                        }
		            }
                    ackerman_d_fk(twist.linear.x, twist.angular.z);

                    ext_set_txd(EXT_DTX_1, Vx[0]);
                    ext_set_txd(EXT_DTX_2, Vx[1]);
                    ext_set_txd(EXT_DTX_3, Vx[2]);
                    ext_set_txd(EXT_DTX_4, Vx[3]);

                    ext_set_txd(EXT_DTX_5, rad2deg(Wz[0]));
                    ext_set_txd(EXT_DTX_6, rad2deg(Wz[1]));
                    ext_set_txd(EXT_DTX_7, rad2deg(Wz[2]));
                    ext_set_txd(EXT_DTX_8, rad2deg(Wz[3]));
                }
                else if ((twist.linear.x==0)&&(fabs(twist.angular.z)))
                {
                    if(((motor[4].fActPOS<(ROTATION-1)*-1)&&(motor[4].fActPOS>(ROTATION+1)*-1))&&
                        ((motor[5].fActPOS>(ROTATION-1))&&(motor[5].fActPOS<(ROTATION+1)))&&
                        ((motor[6].fActPOS>(ROTATION-1))&&(motor[6].fActPOS<(ROTATION+1)))&&
                        ((motor[7].fActPOS<(ROTATION-1)*-1)&&(motor[7].fActPOS>(ROTATION+1)*-1))
                        )
                    {
                        ext_set_txd(EXT_DTX_1, twist.angular.z*(-1));
                        ext_set_txd(EXT_DTX_2, twist.angular.z);
                        ext_set_txd(EXT_DTX_3, twist.angular.z*(-1));
                        ext_set_txd(EXT_DTX_4, twist.angular.z);
                    }
                     
                    ext_set_txd(EXT_DTX_5, ROTATION*(-1));
                    ext_set_txd(EXT_DTX_6, ROTATION);
                    ext_set_txd(EXT_DTX_7, ROTATION);
                    ext_set_txd(EXT_DTX_8, ROTATION*(-1)); 
                }
                else
                {
                    ext_set_txd(EXT_DTX_1, twist.linear.x);
                    ext_set_txd(EXT_DTX_2, twist.linear.x);
                    ext_set_txd(EXT_DTX_3, twist.linear.x);
                    ext_set_txd(EXT_DTX_4, twist.linear.x);

                    ext_set_txd(EXT_DTX_5, rad2deg(twist.angular.z));
                    ext_set_txd(EXT_DTX_6, rad2deg(twist.angular.z));
                    ext_set_txd(EXT_DTX_7, rad2deg(twist.angular.z));
                    ext_set_txd(EXT_DTX_8, rad2deg(twist.angular.z)); 
                }
                break;
            default:
                break;
        }

        

        for(i = APP_NULL; i < (TXD_MAX-EXT_DATA_WIDTH); i++)
        {
            cs+=ext_txd[i];
        }
        ext_txd[EXT_PTCL_C_S] = (uint8_t)cs;
        ext_txd[EXT_PTCL_C_S+1] = (uint8_t)(cs>>8);
        ser.write(ext_txd,TXD_MAX);

        ser.read(ext_rxd, 1);
        if (ext_rxd[0] == EXT_FRAME_HEADER) 
        {
            ser.read(&ext_rxd[1], RXD_MAX-1);
#if 0
	printf("rxd:[");
	for(i=0;i<RXD_MAX;i++)
	{
		printf(" %02x ", ext_rxd[i]);
	}
	printf("]\r\n");
#endif
            if(ext_chkmsg(ext_rxd))
            {     
                motor[0].fActSPD = ext_get_rxd(EXT_DTX_1);
                motor[1].fActSPD = ext_get_rxd(EXT_DTX_2);
                motor[2].fActSPD = ext_get_rxd(EXT_DTX_3);
                motor[3].fActSPD = ext_get_rxd(EXT_DTX_4);
                motor[4].fActSPD = APP_F_NULL;
                motor[5].fActSPD = APP_F_NULL;
                motor[6].fActSPD = APP_F_NULL;
                motor[7].fActSPD = APP_F_NULL;

                motor[0].fActPOS = APP_F_NULL;
                motor[1].fActPOS = APP_F_NULL;
                motor[2].fActPOS = APP_F_NULL;
                motor[3].fActPOS = APP_F_NULL;
                motor[4].fActPOS = ext_get_rxd(EXT_DTX_5);
                motor[5].fActPOS = ext_get_rxd(EXT_DTX_6);
                motor[6].fActPOS = ext_get_rxd(EXT_DTX_7);
                motor[7].fActPOS = ext_get_rxd(EXT_DTX_8);

                joint_state.header.stamp = ros::Time::now();
                for(i=APP_NULL; i<JOINT_MAX; i++)
                {
                    joint_state.position[i] = deg2rad(motor[i].fActPOS);  
                    joint_state.velocity[i] = motor[i].fActSPD;
                }
                pub_joints.publish(joint_state);

                //
                battery.header.stamp = ros::Time::now();
                battery.voltage = ext_get_rxd(EXT_DTX_9);
                battery.current = 0;
                battery.design_capacity = 40;
	            if(battery.voltage)
                {
                    battery.percentage = (battery.voltage-MIN_BATTERY_VOLTAGE)/(MAX_BATTERY_VOLTAGE-MIN_BATTERY_VOLTAGE);
	            }
                else
                {
                    battery.percentage = 0;
                }
                battery.power_supply_status = 0;
                battery.power_supply_health = 0;
                battery.power_supply_technology = 2;
                battery.present = 1;
                battery.capacity = battery.design_capacity*0.8*battery.percentage;
                pub_battery.publish(battery);

                //
                status.data = ext_rxd[EXT_PTCL_STE];
                pub_status.publish(status);
            }
        }
        ros::spinOnce();
        loop_rate.sleep();
    }

    if(ser.isOpen())
    {
        //motor_stop();
        ser.close();
    }
  
    return 0;
}
